#include "BearingOnlyParticleFilter.h"

#include <cassert>

#define TRANSITION_MODEL_STD_XY 	0.03f
#define TRANSITION_MODEL_STD_VXY 	0.20f

void CBearingOnlyParticleFilter::setParticleDataPredictor(CParticleDataPredictor<CParticleVehicleData>* pPredictor) {
	assert(pPredictor != NULL);
	this->pParticleDataPredictor = pPredictor;
}

void CBearingOnlyParticleFilter::setParticleDataWeightCalculator(CImageLandmarksVehicleWeightCalculator* pWeightCalculator) {
	assert(pWeightCalculator != NULL);
	this->pWeightCalculator = pWeightCalculator;
}

/** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
*  This method has additional configuration parameters in "options".
*  Performs the update stage of the RBPF, using the sensed Sensorial Frame:
*
*   \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
*   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
*
* \sa options
*/
void  CBearingOnlyParticleFilter::prediction_and_update_pfStandardProposal(
		const mrpt::slam::CActionCollection	* action,
		const mrpt::slam::CSensoryFrame		* observation,
		const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
{
	size_t i,N = m_particles.size();

	// prediction:
	for (i=0;i<N;i++) {
		pParticleDataPredictor->predict(m_particles[i].d);
	}

	CObservationImageLandmarksPtr obs = observation->getObservationByClass<CObservationImageLandmarks>();
	ASSERT_(obs);

	// Update weights
	for ( i = 0; i < N; i++) {
		m_particles[i].log_w  = pWeightCalculator->compute(m_particles[i].d, obs.pointer());
	}

	// Resample is automatically performed by CParticleFilter when required.
}

void  CBearingOnlyParticleFilter::initializeParticles(size_t  M)
{
	CParticleList::iterator		it;

	clearParticles();
	m_particles.resize(M);
	for (it=m_particles.begin();it!=m_particles.end();it++)
		it->d = new CParticleVehicleData();

	for (it=m_particles.begin();it!=m_particles.end();it++) {
		(*it).d->x  = mrpt::random::RandomUni( initialVehicleData.x - 2.0f, initialVehicleData.x + 2.0f );
		(*it).d->y  = mrpt::random::RandomUni( initialVehicleData.y - 2.0f, initialVehicleData.y + 2.0f );
		(*it).d->w  = mrpt::random::RandomUni( initialVehicleData.w - DEG2RAD(20.0f), initialVehicleData.w + DEG2RAD(20.0f) );

		(*it).d->vx = mrpt::random::RandomNormal( -initialVehicleData.vx, 0.2f );
		(*it).d->vy = mrpt::random::RandomNormal( 0, 0.2f );
		(*it).d->vw = mrpt::random::RandomNormal( 0, DEG2RAD(10.0f) );

		it->log_w	= 0;
	}

}

/** Computes the average velocity
  */
void CBearingOnlyParticleFilter::getMean( float &x, float &y, float &w, float &vx, float &vy, float &vw )
{
	CParticleList::iterator		it;

	double sumW=0;
	for (it=m_particles.begin();it!=m_particles.end();it++)
		sumW+=exp( it->log_w );

	ASSERT_(sumW>0)

	x = y = w = vx = vy = vw = 0;

	for (it=m_particles.begin();it!=m_particles.end();it++)
	{
		double wei = exp(it->log_w) / sumW;

		x += (float)wei * (*it).d->x;
		y += (float)wei * (*it).d->y;
		w += (float)wei * (*it).d->w;
		vx+= (float)wei * (*it).d->vx;
		vy+= (float)wei * (*it).d->vy;
		vw+= (float)wei * (*it).d->vw;

		it->log_w	= 0;
	}
}

